#include <cmath>
#include <iostream>
#include <queue>
#include <utility>
#include <list>
#include "fmm.h"

using namespace std;
//#define __DEBUG


bool acute_local_solver (double T_A, double T_B, double a, double b, double c, double f_C, double & T_C)
{
    if (fabs (T_B - T_A) <= c * f_C)
    {
        double theta = asin ((T_B - T_A)/ (c * f_C));
        double beta = acos ((a * a + c * c - b * b) / (2.0 * a * c));
        double alpha = acos ((b * b+ c * c - a * a ) / (2.0 * b * c));

        if (max (0.0, beta - PI / 2.0) <= theta && (PI/2.0 - alpha) >= theta ||
                (beta - PI / 2.0) <= theta && min (0.0, PI/2.0 - alpha) >= theta ) 
        {
            double CP = a * sin (beta - theta);
            double CQ = b * sin (alpha+ theta);

           T_C = 0.5 * (CP * f_C + T_B) + 0.5 * (CQ * f_C + T_A);
        }
        else
        {
            T_C = min (T_A + b * f_C, T_B + a * f_C);
        }
    }
    else
    {
        T_C = min (T_A + b * f_C, T_B + a * f_C);
    }
    return true;
}

double edge (double e1, double e2, double theta)
{
	return sqrt (e1*e1 + e2*e2 - 2.0f * e1*e2*cos (theta));
}
//We know TA, TB, and try to comput TC. We found the neighbor D of A. We flip the triangles around A
double flip(double AB, double BC, double AC, double AD, double BD)
{
	double theta1 = angle (BC, AC, AB);
	double theta2 = angle (BD, AB, AD);
//	cout<<"Theta1"<<theta1 / PI * 180 <<endl;
//	cout<<"Theta2"<<theta2 / PI * 180 <<endl;
	return edge (AC, AD, theta1 + theta2);
}
//compute the angle of C
double angle (double AB, double BC, double AC)
{
	return acos ((AC*AC + BC*BC - AB * AB)/(2.0f * AC * BC));
}

//The trianlge ABC, is marked in clockwise order.
bool update_flip_around_A (Mesh & mesh, Mesh::VertexHandle va, Mesh::VertexHandle vb, Mesh::VertexHandle vc, double fc, double & tc)
{
	Mesh::VertexVertexIter vv_it, vv_it_begin;

#ifdef __DEBUG
	cout<<"Flip Around A: "<<va.idx()<<endl;
	cout<<"B: "<<vb.idx()<<endl;
	
	cout<<"The neighbor around "<<va.idx() ;
	for (vv_it=mesh.vv_iter( va ); vv_it; ++vv_it)
	{
		cout<<"("<<vv_it.handle().idx()<<','<<mesh.data(vv_it).get_distance();
		switch (mesh.data(vv_it).get_state())
		{
		case FAR: cout<<" FAR"; break;
		case CLOSE: cout<<" CLOSE"; break;
		case ALIVE: cout<<" ALIVE"; break;
		}
		cout<<") ";
	}
	cout<<endl;
#endif
	
	//Locate the circulator of B
	for (vv_it=mesh.vv_iter( va ); vv_it; ++vv_it)
	{
		if (vv_it.handle() == vc)
		{
			vv_it_begin = vv_it; 
			break;
		}
	}
	
	//Flip from the circulator  in clockwise direction
	vv_it = vv_it_begin;
	--vv_it;

	Mesh::Point A = mesh.point(va);
	Mesh::Point B = mesh.point(vb);
	Mesh::Point C = mesh.point(vc);
	Mesh::Point D ;

	double AB = (B-A).length();
	double AC = (C-A).length();
	double BC = (C-B).length();
	double AD, BD;
	
	do 
	{
#ifdef __DEBUG
		cout<<"FLIP "<<vv_it.handle().idx()<<','<<mesh.data(vv_it.handle()).get_state()<<endl;
#endif
		D = mesh.point(vv_it.handle());
		AD = (D-A).length();
		BD = (D-B).length();

		BC = flip(AB,BC,AC, AD, BD);
		AB = AD;
		B = D;

		//std::cout<<"A: "<<A<<std::endl;
		//std::cout<<"B: "<<B<<std::endl;
		//std::cout<<"C: "<<C<<std::endl;
		double theta = angle (AB, BC, AC);
		//cout<<"Angle "<<theta / PI * 180.0f<<" "<<fabs(theta - PI/2.0f)<<std::endl;
		if ( theta - PI/2.0f < ERROR && ALIVE == mesh.data(vv_it).get_state() )
		{
			//std::cout<<"VA: "<<mesh.data(va).get_distance()<<std::endl;
			//std::cout<<"VB: "<<mesh.data(vv_it).get_distance()<<std::endl;
			acute_local_solver(mesh.data(va).get_distance(), mesh.data(vv_it).get_distance(), BC, AC, AB, fc, tc);
			//std::cout<<"VC: "<<tc<<std::endl;
			return true;
		}
		//std::cout<<"Infinite Loop"<<std::endl;
		--vv_it;
	}
	while(vv_it.handle().idx() != vv_it_begin.handle().idx());
	return false;
}
bool update_flip_around_B (Mesh & mesh, Mesh::VertexHandle va, Mesh::VertexHandle vb, Mesh::VertexHandle vc, double fc, double & tc)
{
	


	Mesh::VertexVertexIter vv_it,  vv_it_begin;
	//Locate where va is.
#ifdef __DEBUG
	cout<<"Flip Around B"<<endl;
	cout<<"The neighbor around "<<vb.idx() ;
	for (vv_it=mesh.vv_iter( vb ); vv_it; ++vv_it)
	{
		cout<<"("<<vv_it.handle().idx()<<','<<mesh.data(vv_it).get_distance();
		switch (mesh.data(vv_it).get_state())
		{
		case FAR: cout<<" FAR"; break;
		case CLOSE: cout<<" CLOSE"; break;
		case ALIVE: cout<<" ALIVE"; break;
		}
		cout<<") ";
	}
	cout<<endl;
#endif

	Mesh::VertexVertexIter vv_it_a;
	for (vv_it=mesh.vv_iter( vb ); vv_it; ++vv_it)
	{
		if (vv_it.handle() == vc)
		{
			vv_it_begin = vv_it; 
			break;
		}
	}

	Mesh::Point A = mesh.point(va);
	Mesh::Point B = mesh.point(vb);
	Mesh::Point C = mesh.point(vc);
	Mesh::Point D ;

	double AB = (B-A).length();
	double AC = (C-A).length();
	double BC = (C-B).length();
	double AD, BD;

	//Flip from the circulator A in counter-clockwise direction
	
	vv_it = vv_it_begin;
	vv_it_begin;
	++vv_it;

	A = mesh.point(va);
	B = mesh.point(vb);
	C = mesh.point(vc);
	//D ;

	AB = (B-A).length();
	AC = (C-A).length();
	BC = (C-B).length();
	//AD, BD;

	do 
	{
#ifdef DEBUG
		cout<<"FLIP "<<vv_it.handle().idx()<<','<<mesh.data(vv_it.handle()).get_state()<<endl;
#endif
		D = mesh.point(vv_it.handle());
		AD = (D-A).length();
		BD = (D-B).length();

		AC = flip(AB,AC,BC, BD, AD);
		AB = BD;
		A = D;

		double theta = angle (AB, BC, AC);
		//cout<<"Angle "<<theta / PI * 180.0f<<endl;
		if (theta <= PI/2.0f && ALIVE == mesh.data(vv_it).get_state() )
		{
			acute_local_solver(mesh.data(vv_it).get_distance(), mesh.data(vb).get_distance(), BC, AC, AB, fc, tc);
			return true;
		}
		++vv_it;
	}
	while(vv_it.handle().idx() != vv_it_begin.handle().idx());
	return false;
}

bool update_in_line (Mesh & mesh, Mesh::VertexHandle va, Mesh::VertexHandle vc, double & dis_c)
{
	//In this function. We first try to flip. If flip fails, we continue to update in line.
	//cout<<vc.idx()<<" Update In Line"<<endl;

	double x = mesh.point(va)[0] - mesh.point(vc)[0];
	double y = mesh.point(va)[1] - mesh.point(vc)[1];
	double z = mesh.point(va)[2] - mesh.point(vc)[2];

	 dis_c = mesh.data(va).get_distance() +sqrt (x*x + y*y + z*z);

	return true;
}
bool update (Mesh & mesh, const Mesh::VertexHandle & v_handle)
{
#ifdef __DEBUG
	std::cout<<"Update Vertex "<<v_handle.idx()<<std::endl;
#endif
	//Search the vertex handle around to find tow points which have alread been updated.
	Mesh::VertexVertexIter vv_it, vv_it_next, vv_it_prev, vv_it_begin;
	vv_it_begin = mesh.vv_iter( v_handle );
/*
	for (vv_it=mesh.vv_iter( v_handle ); vv_it; ++vv_it)
	{
		vv_it_next = vv_it; 
		++vv_it_next;
		if (ALIVE == mesh.data(vv_it).get_state()  && 
			ALIVE == mesh.data(vv_it_next).get_state())
		{
			//std::cout<<"Update by triangle"<<std::endl;
			update_in_triangle (mesh, vv_it.handle(), vv_it_next.handle(), v_handle);
			return true;
		}
	}
*/


	double dis_result  = -1; 
	double dis_c = -1;
	//Update by line;
	//Find one ALIVE vertex to update by a line. For examples, at the initila points. 
	// A Bug low the performance down. When we upfolding the triangle, it cannot overlap with all the previous triangles. FIXME Later
	
	//
	for (vv_it=mesh.vv_iter( v_handle ); vv_it; ++vv_it)
	{
		//Passive update scheme
		if (ALIVE == mesh.data(vv_it).get_state())
		{
			vv_it_next = vv_it;
			vv_it_prev = vv_it;
			++vv_it_next;
			--vv_it_prev;
			if (update_flip_around_A(mesh, vv_it.handle(), vv_it_next.handle(), v_handle, 1, dis_c)) 
			{
				//break;
				
				if (dis_result < 0 || dis_c < dis_result)
				{
					dis_result = dis_c;
				}
				
			}
			if (update_flip_around_B(mesh, vv_it_prev.handle(), vv_it.handle(), v_handle, 1, dis_c))
			{
				//break;
				
				if (dis_result < 0 || dis_c < dis_result)
				{
					dis_result = dis_c;
				}
				
			}
			if (update_in_line (mesh, vv_it.handle(),  v_handle, dis_c)) 
			{
				//break;
				
				if (dis_result < 0 || dis_c < dis_result)
				{
					dis_result = dis_c;
				}
				
			}
		}
	}
	//dis_result = dis_c;
	if (dis_result < 0) return false;
	if (mesh.data(v_handle).get_distance() < 0 || mesh.data(v_handle).get_distance() > dis_result)
	{
		mesh.data(v_handle).set_distance(dis_result);
	}
	//cout<<"Vertex Updated Value is "<<mesh.data(v_handle).get_distance() <<endl;
	return true;
}
int fast_marching (Mesh & mesh, std::vector<Mesh::VertexHandle> & boundary)
{
	//This heap is used to extract the vertexhandle of the least distance
	std::list <VertexQueueElement> heap;

	for (unsigned int i = 0; i < boundary.size(); ++i)
	{
		//Add all the handles into the heap
		heap.push_back(VertexQueueElement(boundary[i], mesh.data(boundary[i]).get_distance()));
		//std::cout<<"Boundary value"<< mesh.data(boundary[i]).get_distance()<<std::endl;
		//std::cout<<"idx "<<boundary[i].idx()<<std::endl;
		//Set the status to be CLOSE
		mesh.data(boundary[i]).set_state(CLOSE);
	}
	while (0 != heap.size() )
	{
		//std::cout<<"Heap Size"<<heap.size()<<std::endl;

		Mesh::VertexHandle v_min ;
		double dis = -1;
		std::list<VertexQueueElement>::iterator index;
		for (std::list<VertexQueueElement>::iterator it = heap.begin(); it != heap.end(); ++it)
		{
			if (dis < 0 || dis > mesh.data(it->v_handle).get_distance())
			{
				v_min = it->v_handle;
				dis = mesh.data(it->v_handle).get_distance();
				index = it;
			}
			//std::cout<<'('<<it->v_handle.idx()<<',';
			//std::cout<<mesh.data(it->v_handle).get_distance()<<')';
			
		}
		//std::cout<<std::endl;

		//std::cout<<"Extract..."<<v_min.idx()<<std::endl;
		mesh.data(v_min).set_state(ALIVE);
		heap.erase(index);


		//Update all the neighbors of v_min;
		Mesh::VertexVertexIter vv_it;
		for (vv_it=mesh.vv_iter( v_min ); vv_it; ++vv_it)
		{
			//Update all its neighbors which are not alive 
			if (ALIVE != mesh.data(vv_it).get_state() )
			{	
				//Update the value of this vertex; Both CLOSE and FAR
				update (mesh, vv_it.handle());

				//Add it into the CLOSE heap;
				if (FAR == mesh.data(vv_it).get_state())
				{
					mesh.data(vv_it).set_state(CLOSE);
					heap.push_back(VertexQueueElement(vv_it.handle(), mesh.data(vv_it).get_distance()));
				}
			}
		}
	}
	return 0;
}

void face_point_fmm (Mesh & mesh, int face_idx, Mesh::Point & p)
{

	Mesh::FaceHandle start_face = mesh.face_handle(face_idx);
	std::vector<Mesh::VertexHandle> boundary;
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(start_face); fv_it; ++fv_it)
	{
		double length = (p - mesh.point(fv_it)).length();
		//std::cout<<"Length "<<length<<std::endl;
		//std::cout<<fv_it.handle().idx()<<std::endl;
		mesh.data(fv_it.handle()).set_distance (length);
		mesh.data(fv_it.handle()).set_state(ALIVE);
		boundary.push_back(fv_it.handle());
	}
	fast_marching(mesh, boundary);
}

